#!/usr/bin/env python3

# import rospy
# from mavros_msgs.srv import CommandLong

# def send_gimbal_control(pitch, roll, yaw):
#     rospy.wait_for_service('/mavros/cmd/command')
#     try:
#         command_service = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
#         # MAV_CMD_DO_MOUNT_CONTROL = 205
#         response = command_service(
#             command=205,  # MAV_CMD_DO_MOUNT_CONTROL
#             param1=pitch,  # Pitch angle in degrees
#             param2=roll,   # Roll angle in degrees
#             param3=yaw,    # Yaw angle in degrees
#             param4=0,      # Unused
#             param5=0,      # Unused
#             param6=0,      # Unused
#             param7=0       # MAV_MOUNT_MODE_MAVLINK_TARGETING
#         )
#         if response.success:
#             rospy.loginfo("Gimbal control command sent successfully!")
#         else:
#             rospy.logwarn("Failed to send Gimbal control command.")
#     except rospy.ServiceException as e:
#         rospy.logerr(f"Service call failed: {e}")

# if __name__ == '__main__':
#     rospy.init_node('gimbal_control_node', anonymous=True)
#     # Example: Set pitch to 10°, roll to 0°, yaw to 20°
#     send_gimbal_control(pitch=0, roll=0, yaw=0)
import rospy
from mavros_msgs.msg import Mavlink

def mount_status_callback(msg):
    if msg.msgid == :  # MOUNT_STATUS
        rospy.loginfo(f"Received MOUNT_STATUS: {msg.payload64}")

if __name__ == '__main__':
    rospy.init_node('mount_status_node', anonymous=True)
    rospy.Subscriber("/mavlink/from", Mavlink, mount_status_callback)
    rospy.spin()